from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution


def generate_launch_description():
    # 1) 机器人基础 bringup
    bringup_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [
                    FindPackageShare("my_robot_bringup"), 
                    "launch",
                    "my_robot_m.launch.py",
                ]
            )
        )
    )

    # 2) Servo 功能
    servo_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [
                    FindPackageShare("my_robot_bringup"),
                    "launch",
                    "zc4_servo.launch.py",
                ]
            )
        )
    )

    return LaunchDescription(
        [
            bringup_launch,
            servo_launch,
        ]
    )
